﻿/*
    Copyright 2010, 2011 Eric Wong
	contact@optimalcycling.com
	http://www.optimalcycling.com
	Optimal Cycling - Advanced power pacing program for cyclists
	
	This file is part of Optimal Cycling.

    Optimal Cycling is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Optimal Cycling is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with Optimal Cycling.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _FILE_VECTOR_H
#define _FILE_VECTOR_H

#ifndef __OPENCL_VERSION__
	#include "Options.h"
	#include "Constants.h"
#endif

#define overMax(currVal, maxVal) ( (currVal) <= (maxVal) ? 0 : ((currVal) - (maxVal)) )

inline void Real4DeepCopyPrivate(const real4* origin, real4* dest)
{
	(*dest).s0 = (*origin).s0;
	(*dest).s1 = (*origin).s1;
	(*dest).s2 = (*origin).s2;
	(*dest).s3 = (*origin).s3;
}

inline void Real8DeepCopyPrivate(const real8* origin, real8* dest)
{
	(*dest).s0 = (*origin).s0;
	(*dest).s1 = (*origin).s1;
	(*dest).s2 = (*origin).s2;
	(*dest).s3 = (*origin).s3;
	(*dest).s4 = (*origin).s4;
	(*dest).s5 = (*origin).s5;
	(*dest).s6 = (*origin).s6;
	(*dest).s7 = (*origin).s7;
}

inline void Real4DeepCopy(__global const real4* origin, __global real4* dest)
{
	(*dest).s0 = (*origin).s0;
	(*dest).s1 = (*origin).s1;
	(*dest).s2 = (*origin).s2;
	(*dest).s3 = (*origin).s3;
}

inline void Real8DeepCopy(__global const real8* origin, __global real8* dest)
{
	(*dest).s0 = (*origin).s0;
	(*dest).s1 = (*origin).s1;
	(*dest).s2 = (*origin).s2;
	(*dest).s3 = (*origin).s3;
	(*dest).s4 = (*origin).s4;
	(*dest).s5 = (*origin).s5;
	(*dest).s6 = (*origin).s6;
	(*dest).s7 = (*origin).s7;
}

//inline void Real4Sub(__global real4* vals,  __global const real4* subtractVals)
//{
//	(*vals).s0 = (*vals).s0 - (*subtractVals).s0;
//	(*vals).s1 = (*vals).s1 - (*subtractVals).s1;
//	(*vals).s2 = (*vals).s2 - (*subtractVals).s2;
//	(*vals).s3 = (*vals).s3 - (*subtractVals).s3;
//}
//
//inline void Real4Sub2(__global real4* vals, __global const real4* firstVals, __global const real4* subtractVals)
//{
//	(*vals).s0 = (*firstVals).s0 - (*subtractVals).s0;
//	(*vals).s1 = (*firstVals).s1 - (*subtractVals).s1;
//	(*vals).s2 = (*firstVals).s2 - (*subtractVals).s2;
//	(*vals).s3 = (*firstVals).s3 - (*subtractVals).s3;
//}

//inline real Real4Grade(__global const real4* vals)
//{
//    real run = sqrt((*vals).s0 * (*vals).s0 + (*vals).s2 * (*vals).s2);
//
//	if (run < SMALL_REAL)
//	{
//		return LARGE_REAL;
//	}
//	else
//		return (*vals).s1 / run;	
//}

inline void ArrayAdd(__global real* vals, __global const real* additive, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		vals[i] = vals[i] + additive[i];
	}
}

inline void ArrayCopy(__global const real* origin, __global real* dest, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		dest[i] = origin[i];
	}
}

inline void ArrayFill(const real fillValue, __global real* dest, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		dest[i] = fillValue;
	}
}

inline void Real4ArrayCopy(__global const real4* origin, __global real4* dest, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		(*dest).s0 = (*origin).s0;
		(*dest).s1 = (*origin).s1;
		(*dest).s2 = (*origin).s2;
		(*dest).s3 = (*origin).s3;
	}
}

inline void Real8ArrayCopy(__global const real8* origin, __global real8* dest, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		(*dest).s0 = (*origin).s0;
		(*dest).s1 = (*origin).s1;
		(*dest).s2 = (*origin).s2;
		(*dest).s3 = (*origin).s3;
		(*dest).s4 = (*origin).s4;
		(*dest).s5 = (*origin).s5;
		(*dest).s6 = (*origin).s6;
		(*dest).s7 = (*origin).s7;
	}
}

inline void ArraySub(__global real* vals, __global const real* subtractVals, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		vals[i] = vals[i] - subtractVals[i];
	}
}

inline void ArrayMult(__global real* vals, const real multFactor, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		vals[i] = vals[i] * multFactor;
	}
}

inline void ArrayZero(__global real* vals, const sizeT numPoints)
{
	sizeT i;
    for (i = 0; i < numPoints; i++)
    {
		vals[i] = 0;
	}
}

inline void KahanSum(real* sum, real* correction, real addVal)
{
	real corrected_next_term, new_sum;

	corrected_next_term = addVal - (*correction);
	new_sum = (*sum) + corrected_next_term;
	(*correction) = (new_sum - (*sum)) - corrected_next_term;
	(*sum) = new_sum;
}

/// <summary>
/// Holds the params for the butterworth filter
/// </summary>
typedef struct 
{
	// Butterworth params
	real bwCutOffF;
	real bwDataCollectF;
	real bwW;
	real bwK1;
	real bwK2;
	real bwK3;
	real bwA0;
	real bwA1;
	real bwA2;
	real bwB1;
	real bwB2;

} BwParams;

/// <summary>
/// Butterworth filter
/// </summary>
inline void BWSmoothData(real bwCutOffF, real bwDataCollectF, sizeT smoothNum, sizeT stratIndexer, __global real* powerTempSpace, 
							  __global real* childPowerGenes, __global const real2* powerLimits, int recopyFirstAndLast)
{
	BwParams smootherBwParams;
	real2 tempSmoothVals;
	sizeT counter;
	
	smootherBwParams.bwCutOffF = bwCutOffF;
	smootherBwParams.bwDataCollectF = bwDataCollectF;

	smootherBwParams.bwW = real_tan(PI * smootherBwParams.bwCutOffF / smootherBwParams.bwDataCollectF) / 0.802f;
	smootherBwParams.bwK1 = SQRT_TWO * smootherBwParams.bwW;
	smootherBwParams.bwK2 = pow(smootherBwParams.bwW, TWO);
	smootherBwParams.bwA0 = smootherBwParams.bwK2 / (ONE + smootherBwParams.bwK1 + smootherBwParams.bwK2);
	smootherBwParams.bwA1 = TWO * smootherBwParams.bwA0;
	smootherBwParams.bwA2 = smootherBwParams.bwA0;
	smootherBwParams.bwK3 = smootherBwParams.bwA1 / smootherBwParams.bwK2;

	smootherBwParams.bwB1 = NEG_TWO * smootherBwParams.bwA0 + smootherBwParams.bwK3;
	smootherBwParams.bwB2 = ONE - TWO * smootherBwParams.bwA0 - smootherBwParams.bwK3;

	// First pass
	tempSmoothVals.s0 = childPowerGenes[stratIndexer + smoothNum - 2];
	tempSmoothVals.s1 = childPowerGenes[stratIndexer + smoothNum - 1];
	powerTempSpace[stratIndexer + 0] = childPowerGenes[stratIndexer + 0]; 
	powerTempSpace[stratIndexer + 1] = childPowerGenes[stratIndexer + 1]; 
	for (counter = 2; counter < smoothNum; counter++)
	{			
		powerTempSpace[stratIndexer + counter] = smootherBwParams.bwA0 * childPowerGenes[stratIndexer + counter] + 
			smootherBwParams.bwA1 * childPowerGenes[stratIndexer + counter - 1] +
			smootherBwParams.bwA2 * childPowerGenes[stratIndexer + counter - 2] +
			smootherBwParams.bwB1 * powerTempSpace[stratIndexer + counter - 1] +
			smootherBwParams.bwB2 * powerTempSpace[stratIndexer + counter - 2];
	}

	// Second pass
	// Last two points specially copied
	childPowerGenes[stratIndexer + smoothNum - 2] = powerTempSpace[stratIndexer + smoothNum - 2]; 
	childPowerGenes[stratIndexer + smoothNum - 1] = powerTempSpace[stratIndexer + smoothNum - 1]; 
	for (counter = smoothNum - 1 - 2; counter >= 1; counter--)
	{			
		childPowerGenes[stratIndexer + counter] = clamp(smootherBwParams.bwA0 * powerTempSpace[stratIndexer + counter] + 
			smootherBwParams.bwA1 * powerTempSpace[stratIndexer + counter + 1] +
			smootherBwParams.bwA2 * powerTempSpace[stratIndexer + counter + 2] +
			smootherBwParams.bwB1 * childPowerGenes[stratIndexer + counter + 1] +
			smootherBwParams.bwB2 * childPowerGenes[stratIndexer + counter + 2], 
			powerLimits[stratIndexer + counter].s0, powerLimits[stratIndexer + counter].s1);

		if (counter == 0)	// since counter is unsigned
			break;
	}

	if (recopyFirstAndLast)
	{
		// First two points specially copied
		childPowerGenes[stratIndexer + 0] = clamp(powerTempSpace[stratIndexer + 0], 
			powerLimits[stratIndexer + 0].s0, powerLimits[stratIndexer + 0].s1); 
		childPowerGenes[stratIndexer + 1] = clamp(powerTempSpace[stratIndexer + 1], 
			powerLimits[stratIndexer + 1].s0, powerLimits[stratIndexer + 1].s1); 

		// Last two points specially copied
		childPowerGenes[stratIndexer + smoothNum - 2] = clamp(tempSmoothVals.s0, 
			powerLimits[stratIndexer + smoothNum - 2].s0, powerLimits[stratIndexer + smoothNum - 2].s1); 
		childPowerGenes[stratIndexer + smoothNum - 1] = clamp(tempSmoothVals.s1, 
			powerLimits[stratIndexer + smoothNum - 1].s0, powerLimits[stratIndexer + smoothNum - 1].s1); 
	}
}

#endif		// _FILE_VECTOR_H


